#include "flight_tool.h"
#include "flight_controller.h"

geometry_msgs::PoseStamped local_position;
geometry_msgs::PoseStamped target_position;

FlightController::FlightController()
    : nh()
{
    drone_flight_cmd_sub = nh.subscribe<mavproxy_ros1::FlightCmd>(MAVPROXY_PARSED_CMD, GLOBAL_DEFAULT_QUEUE_SIZE,
                                                                  std::bind(&FlightController::CmdCallback, this, std::placeholders::_1));
    drone_mavproxy_state_sub = nh.subscribe<mavproxy_ros1::MavproxyState>(MAVPROXY_STATE, GLOBAL_DEFAULT_QUEUE_SIZE,
                                                                          std::bind(&FlightController::StateCallback, this, std::placeholders::_1));
    drone_local_positin_sub = nh.subscribe<geometry_msgs::PoseStamped>(MAVROS_INFO_LOCAL_POSE, GLOBAL_DEFAULT_QUEUE_SIZE,
                                                                       std::bind(&FlightController::PositionCallback, this, std::placeholders::_1));

    drone_target_pose_pub = nh.advertise<geometry_msgs::PoseStamped>(MAVROS_SET_LOCAL_POSE, GLOBAL_DEFAULT_QUEUE_SIZE);
    drone_target_gpose_pub = nh.advertise<geographic_msgs::GeoPoseStamped>(MAVROS_SET_GLOABL_POSE, GLOBAL_DEFAULT_QUEUE_SIZE);
    drone_target_velocity_pub = nh.advertise<geometry_msgs::Twist>(MAVROS_SET_VELOCITY, GLOBAL_DEFAULT_QUEUE_SIZE);
    drone_target_attitude_pub = nh.advertise<geometry_msgs::PoseStamped>(MAVROS_SET_ATTITUDE, GLOBAL_DEFAULT_QUEUE_SIZE);
    drone_target_goal_pub = nh.advertise<geometry_msgs::PoseStamped>(MAVPROXY_GOAL, GLOBAL_DEFAULT_QUEUE_SIZE);

    drone_arming_client = nh.serviceClient<mavros_msgs::CommandBool>(MAVROS_SRV_ARM);
    drone_set_home_client = nh.serviceClient<mavros_msgs::CommandHome>(MAVROS_SET_HOME);
    drone_mode_client = nh.serviceClient<mavros_msgs::SetMode>(MAVROS_SRV_MODE);
    drone_takeoff_cmd_client = nh.serviceClient<mavros_msgs::CommandTOL>(MAVROS_SRV_TAKEOFF);
    drone_land_cmd_client = nh.serviceClient<mavros_msgs::CommandTOL>(MAVROS_SRV_LAND);
    drone_clear_mission_client = nh.serviceClient<mavros_msgs::WaypointClear>(MAVROS_CLEAR_MISSION);

    cmd_executers = {
        {CMD_FLIGHT_TAKEOFF, std::bind(&FlightController::LetDroneTakeoff, this)},
        {CMD_FLIGHT_LAND, std::bind(&FlightController::LetDroneLand, this)},
        // {CMD_FLIGHT_HOVER, std::bind(&FlightController::LetDroneLand, this)},
        {CMD_FLIGHT_FLY_POSE, std::bind(&FlightController::LetDroneFlyToTargetPosition, this)},
        {CMD_FLIGHT_FLY_GPOSE, std::bind(&FlightController::LetDroneFlyViaMission, this)},
        {CMD_FLIGHT_MISSION, std::bind(&FlightController::LetDroneFlyViaMission, this)},
        {CMD_FLIGHT_ARM, std::bind(&FlightController::ArmDrone, this, true)},
        {CMD_FLIGHT_DISARM, std::bind(&FlightController::ArmDrone, this, false)},
        {CMD_FLIGHT_MODE, std::bind(&FlightController::SwitcheMode, this)},
        {CMD_FLIGHT_STOP, std::bind(&FlightController::LetDroneStop, this)}};
}

void FlightController::CmdCallback(const mavproxy_ros1::FlightCmd::ConstPtr &msg)
{
    ROS_INFO("\r\n[TD DEBUG][CTR] Cmd get %s", (*msg).cmdtype.c_str());
    drone_current_cmd = *msg;

    if (drone_current_cmd.cmdtype != CMD_FLIGHT_STOP)
        stop_detected = false;

    int ret = HandleCmd();
    ROS_INFO("\r\n[TD DEBUG][CTR] HandleCmd ret = %d", ret);
}

void FlightController::StateCallback(const mavproxy_ros1::MavproxyState::ConstPtr &msg)
{
    mavproxy_current_state = *msg;
}

void FlightController::PositionCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    local_position = (*msg);
}

int FlightController::SwitcheModeByName(const std::string &mode_name)
{
    mavros_msgs::SetMode srv;
    srv.request.custom_mode = mode_name;

    if (drone_mode_client.call(srv) && srv.response.mode_sent) {
        ROS_INFO("\r\n[TD DEBUG][CTR] Drone mode set to %s", mode_name.c_str());
        return RETSUCCESS;
    }

    ROS_ERROR("\r\n[TD DEBUG][CTR] Failed to set drone mode to %s", mode_name.c_str());
    return RETFAILED;
}

int FlightController::SwitcheMode()
{
    if (drone_current_cmd.paramdata.size() < 1) {
        ROS_ERROR("\r\n[TD DEBUG][CTR] Failed to set drone mode, because the mode id not set");
        return RETFAILED;
    }

    std::string mode_name = MODE_MAP[drone_current_cmd.paramdata[0]];
    mavros_msgs::SetMode srv;
    srv.request.custom_mode = mode_name;

    if (drone_mode_client.call(srv) && srv.response.mode_sent) {
        ROS_INFO("\r\n[TD DEBUG][CTR] Drone mode set to %s", mode_name.c_str());
        return RETSUCCESS;
    }

    ROS_ERROR("\r\n[TD DEBUG][CTR] Failed to set drone mode to %s", mode_name.c_str());
    return RETFAILED;
}

int FlightController::ArmDrone(bool arm)
{
    mavros_msgs::CommandBool srv;
    srv.request.value = arm;

    if (obs_enable) {
        drone_target_goal.pose = mavproxy_current_state.local_position;
        drone_target_goal.pose.position.z += 2.5f;
        drone_target_goal_pub.publish(drone_target_goal);
        SwitcheModeByName(OFFBOARD_PX4_MODE);
    }

    if (drone_arming_client.call(srv) && srv.response.success) {
        ROS_INFO("\r\n[TD DEBUG][CTR] Drone %sarmed", arm ? "" : "dis");
        return RETSUCCESS;
    }

    ROS_ERROR("\r\n[TD DEBUG][CTR] Failed to %sarmed", arm ? "" : "dis");
    return RETFAILED;
}

int FlightController::LetDroneTakeoff()
{
    ROS_INFO("\r\n[TD DEBUG][CTR] Enter to let drone takeoff");

    if (mavproxy_current_state.flight_mode != STABILIZE_PX4_MODE) {
        ROS_WARN("\r\n[TD DEBUG][CTR] can not change mode to takeoff");
        return RETFAILED;
    }

    int ret = SwitcheModeByName(TAKEOFF_PX4_MODE);
    if (ret != RETSUCCESS) {
        ROS_WARN("\r\n[TD DEBUG][CTR] Failed to change mode to takeoff.");
        return RETFAILED;
    }

    ROS_INFO("\r\n[TD DEBUG][CTR] Send takeoff operation success.");
    return RETSUCCESS;
}

int FlightController::LetDroneLand()
{
    ROS_INFO("\r\n[TD DEBUG][CTR] Enter to let drone land");

    if (mavproxy_current_state.flight_mode != HOLD_PX4_MODE) {
        ROS_WARN("\r\n[TD DEBUG][CTR] can not change mode to land.");
        return RETFAILED;
    }

    int ret = SwitcheModeByName(LAND_PX4_MODE);
    if (ret != RETSUCCESS) {
        ROS_WARN("\r\n[TD DEBUG][CTR] Failed to change mode to land.");
        return RETFAILED;
    }

    ROS_INFO("\r\n[TD DEBUG][CTR] Send land operation success.");
    return RETSUCCESS;
}

int FlightController::LetDroneFlyToTargetPosition()
{
    ROS_INFO("\r\n[TD DEBUG][CTR] Enter to let drone fly to target position.");
    int ret = GetTargetPlannerGoal();
    if (ret != RETSUCCESS) {
        ROS_INFO("\r\n[TD DEBUG][CTR] Failed to get target local target position.");
        return RETFAILED;
    }

    if (obs_enable) {
        ROS_INFO("\r\n[TD DEBUG][CTR] Enter to handle fly with obs");
        return LetDroneFlyToPosWithObs();
    }

    ROS_INFO("\r\n[TD DEBUG][CTR] Enter to handle fly without obs");
    return LetDroneFlyToPosWithoutObs();
}

int FlightController::LetDroneFlyToPosWithObs()
{
    GetTargetPlannerGoal();

    // 在飞行中切换目标
    if (mavproxy_current_state.flight_mode == OFFBOARD_PX4_MODE) {
        ROS_INFO("\r\n[TD DEBUG][CTR] Change flight target when flying.");
        drone_target_goal_pub.publish(drone_target_goal);
        return RETSUCCESS;
    }

    // 发布目标位置
    drone_target_goal_pub.publish(drone_target_goal);

    // 切换到offboard模式
    if (is_stop_local) {
        return SendForOffbPack();
    }

    ROS_INFO("\r\n[TD DEBUG][CTR] change mode to offboard.");
    int ret = SwitcheModeByName(OFFBOARD_PX4_MODE);
    if (ret != RETSUCCESS) {
        ROS_ERROR("\r\n[TD DEBUG][CTR] Failed to change mode to offboard.");
        return RETFAILED;
    }

    return ret;
}

int FlightController::SendForOffbPack()
{
    ros::Rate rate(GLOBAL_COMMON_ROS_RATE / 2); // 最低2HZ不断连，这里使用10HZ
    ros::Time last_request = ros::Time::now();
    bool time_out = false;

    for (int i = 0; i < 200; i++)
    {
        drone_target_pose_pub.publish(drone_target_goal);
        SwitcheModeByName(OFFBOARD_PX4_MODE);
        if (mavproxy_current_state.flight_mode == OFFBOARD_PX4_MODE){
            return 0;
        }
    }
    ROS_ERROR("\r\n[TD DEBUG][CTR] Failed to change mode to offboard");
    return 1;
}

int FlightController::LetDroneFlyToPosWithoutObs()
{
    ROS_INFO("\r\n[TD DEBUG][CTR] Enter to let drone fly to target position.");

    ros::Rate rate(GLOBAL_COMMON_ROS_RATE / 2); // 最低2HZ不断连，这里使用10HZ
    ros::Time last_request = ros::Time::now();
    bool time_out = false;
    bool is_reached = false;

    while (ros::ok() && !is_reached && !stop_detected)
    {
        if (stop_detected) {
            return RETFAILED;
        }
        if (FlightTool::GetInstance()->GetDistance(mavproxy_current_state.local_position, drone_target_goal.pose) < GLOBAL_REACHED_DISTANCE) {
            ROS_INFO("\r\n[TD DEBUG][CTR] Drone arrived target position");
            is_reached = true;
            SwitcheModeByName(HOLD_PX4_MODE);
            return RETSUCCESS;
        }
        if (ros::Time::now() - last_request > ros::Duration(DRONE_FLY_TARGET_TIMEOUT_DURATION * 5)) {
            time_out = true;
            return RETFAILED;
        }
        if (mavproxy_current_state.flight_mode != OFFBOARD_PX4_MODE) {
            SwitcheModeByName(OFFBOARD_PX4_MODE);
        }

        if (obs_enable) {
            drone_target_goal_pub.publish(drone_target_goal);
        } else {
            drone_target_pose_pub.publish(drone_target_goal);
        }
        ros::spinOnce();
        rate.sleep();
    }

    return RETSUCCESS;
}

int FlightController::LetDroneFlyViaMission()
{
    ClearMission();

    mavros_msgs::WaypointPush srv = GetTargetMissionWayPoint();
    if (drone_push_mission_client.call(srv) && srv.response.success) {
        ROS_INFO("\r\n[TD DEBUG][CTR] Push Drone mission success");
        return RETSUCCESS;
    }

    int ret = SwitcheModeByName(MISSION_PX4_MODE);
    if (ret != RETSUCCESS) {
        ROS_WARN("\r\n[TD DEBUG][CTR] Failed to change mode when fly to target position.");
        return RETFAILED;
    }

    ROS_ERROR("\r\n[TD DEBUG][CTR] Failed to push mission to drone mission");
    return RETFAILED;
}

int FlightController::GetTargetPlannerGoal()
{
    if (drone_current_cmd.paramdata.size() > 1) {
        drone_target_goal.pose.position.x = drone_current_cmd.paramdata[0];
        drone_target_goal.pose.position.y = drone_current_cmd.paramdata[1];
        drone_target_goal.pose.position.z = drone_current_cmd.paramdata.size() > 2 ? drone_current_cmd.paramdata[2] : mavproxy_current_state.local_position.position.z;
    } else {
        ROS_INFO("\r\n[TD DEBUG][CTR] Failed to get target local position, error param size [%ld].", drone_current_cmd.paramdata.size());
        return RETFAILED;
    }

    ROS_INFO("\r\n[TD DEBUG][CTR] Getted target goal [%f] [%f] [%f] ", drone_target_goal.pose.position.x, drone_target_goal.pose.position.y, drone_target_goal.pose.position.z);
    return RETSUCCESS;
}

mavros_msgs::WaypointPush FlightController::GetTargetMissionWayPoint()
{
    mavros_msgs::WaypointPush drone_target_wapoints_list;
    drone_target_wapoints_list.request.start_index = 0;

    mavros_msgs::Waypoint target_point;
    target_point.frame = MAV_FRAME_GLOBAL; // 任务模式只支持GPS FRAME
    target_point.command = MAV_CMD_NAV_WAYPOINT;
    target_point.is_current = true;
    target_point.autocontinue = true;
    target_point.param1 = 5;   // 航行点停留时间
    target_point.param2 = 0.5; // 接受半径
    target_point.param3 = 0.0;
    // target_point.param4 = 0.0; 配置成NAN就会启用heading mode 自动朝向行进方向

    if (drone_current_cmd.paramdata.size() > 1) {
        target_point.x_lat = drone_current_cmd.paramdata[0];
        target_point.y_long = drone_current_cmd.paramdata[1];
        target_point.z_alt = drone_current_cmd.paramdata.size() > 2 ? drone_current_cmd.paramdata[2] : mavproxy_current_state.gps_position.latitude;
    }

    drone_target_wapoints_list.request.waypoints.push_back(target_point);
    return drone_target_wapoints_list;
}

int FlightController::ClearMission()
{
    mavros_msgs::WaypointClear srv;

    if (drone_clear_mission_client.call(srv) && srv.response.success) {
        ROS_INFO("\r\n[TD DEBUG][CTR] Clear Drone mission success");
        return RETSUCCESS;
    }
    ROS_INFO("%d", srv.response.success);

    ROS_ERROR("\r\n[TD DEBUG][CTR] Failed to clear drone mission");
    return RETFAILED;
}

// 让无人机设置home点，该位置依靠GPS，故需要在GPS正常时触发
int FlightController::LetDroneSetHome()
{
    ROS_INFO("\r\n[TD DEBUG][CTR] Enter to set home.");

    mavros_msgs::CommandHome drone_target_home_point;
    drone_target_home_point.request.latitude = mavproxy_current_state.gps_position.latitude;
    drone_target_home_point.request.longitude = mavproxy_current_state.gps_position.longitude;
    drone_target_home_point.request.altitude = mavproxy_current_state.gps_position.altitude;

    if (drone_set_home_client.call(drone_target_home_point) && drone_target_home_point.response.success) {
        ROS_INFO("\r\n[TD DEBUG][CTR] Home position set successfully.");
        return RETSUCCESS;
    }

    ROS_ERROR("\r\n[TD DEBUG][CTR] Failed to send set home position cmd.");
    return RETFAILED;
}

int FlightController::LetDroneStop()
{
    stop_detected = true;
    return RETSUCCESS;
}

int FlightController::HandleCmd()
{
    ROS_DEBUG("\r\n[TD DEBUG][CTR] Enter to handle command");

    auto executer = cmd_executers.find(drone_current_cmd.cmdtype);
    if (executer != cmd_executers.end()) {
        ROS_DEBUG("\r\n[TD DEBUG][CTR] Find target command executer.");
    } else {
        ROS_ERROR("\r\n[TD DEBUG][CTR] No handler for command type: %s", drone_current_cmd.cmdtype.c_str());
        return RETFAILED;
    }

    ROS_DEBUG("\r\n[TD DEBUG][CTR] Start to execute the cmd [%s].", drone_current_cmd.cmdtype.c_str());
    int ret = executer->second();
    if (ret != RETSUCCESS) {
        ROS_INFO("\r\n[TD DEBUG][CTR] Execute the cmd failed.");
        return RETFAILED;
    }

    return RETSUCCESS;
}

void FlightController::InitilizeNode()
{
    // 等待飞控和MAVROS建立连接
    ros::Rate rate(GLOBAL_COMMON_ROS_RATE);

    // 检查是否使能避障
    int ret = nh.getParam("/flight_controller/obs_enable", obs_enable);
    if (ret) {
        ROS_INFO("\r\n[TD DEBUG][CTR] Get obs parameter: %d", obs_enable);
    }

    ret = nh.getParam("/goal_controller/is_stop_local", is_stop_local);
    if (ret) {
        ROS_INFO("\r\n[TD DEBUG][CTR] Get stop local parameter: %d", is_stop_local);
    }

    ros::Time last_request = ros::Time::now();
    while (ros::ok() && !mavproxy_current_state.connected && ros::Time::now() - last_request < ros::Duration(DRONE_CONNECTION_TIMEOUT_DURATION)) {
        ros::spinOnce();
        rate.sleep();
    }

    SwitcheModeByName(STABILIZE_PX4_MODE);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "flight_controller");

    FlightController node;
    node.InitilizeNode();
    ros::spin();
    return RETSUCCESS;
}
